import numpy as np
from SE3 import DH, Pose
from robotdriver import robot_driver
from my_robotic_arm_ui import MainWindow
import threading
import time
import sys
from PyQt5.QtWidgets import QApplication, QMainWindow

if __name__ == '__main__':
    theta_1 = 0.0
    theta_2 = -np.pi/2
    theta_3 = 0.0
    theta_4 = 0.0
    theta_5 = 0.0
    theta_6 = 0.0
    joints = [theta_1, theta_2, theta_3, theta_4, theta_5, theta_6]
    robot_driver.move_to_home(joints)
    window = QApplication(sys.argv)
    my_robot_win = MainWindow()
    my_robot_win.show()
    print("============ start my 6-dof robotic arm============")
    sys.exit(window.exec_())